#include "target_detect.hpp"

void local_odom_callback(const nav_msgs::Odometry::Ptr & current_msg)
{
    current_point.x= current_msg->pose.pose.position.x;
    current_point.y= current_msg->pose.pose.position.y;
    current_point.z= current_msg->pose.pose.position.z;
    // current_point.z= current_point_z;

    current_angle.x= current_msg->pose.pose.orientation.x;
    current_angle.y= current_msg->pose.pose.orientation.y;
    current_angle.z= current_msg->pose.pose.orientation.z;
    current_angle.w= current_msg->pose.pose.orientation.w;
    curr_plane_angle = unionsys_core->toEulerAngle(current_angle);
    current_angle_Q = Eigen::Quaterniond(current_msg->pose.pose.orientation.w, current_msg->pose.pose.orientation.x,
                                        current_msg->pose.pose.orientation.y, current_msg->pose.pose.orientation.z);
    local_odom_flag = true;

}

void tag_callback(const swarm_msgs::BoundingBoxes &msg)
{
    std::vector<Box> receive_boxes = deal_detection_to_3dpose(msg, "tag", REALSIZE_tag, FOCUS_downcam, 200, R_downcam);
    if(receive_boxes.size()>0)
    {
        double min_dist_vehicle2box = 5.0;
        Box nearest_box;
        int have_real_box = 0;
        for(int i=0;i<receive_boxes.size();i++)
        {
            Eigen::Vector3d receive_center;
            receive_center(0) = (receive_boxes[i].x_min+receive_boxes[i].x_max)/2.0;
            receive_center(1) = (receive_boxes[i].y_min+receive_boxes[i].y_max)/2.0;
            receive_center(2) = (receive_boxes[i].z_min+receive_boxes[i].z_max)/2.0;
            double dist_tmp = sqrt((receive_center(0)-current_point.x)*(receive_center(0)-current_point.x)+
                                    (receive_center(1)-current_point.y)*(receive_center(1)-current_point.y)+
                                    (receive_center(2)-current_point.z)*(receive_center(2)-current_point.z));
            if(dist_tmp<1.0)
            {
                continue;
            }
            if(dist_tmp<min_dist_vehicle2box)
            {
                min_dist_vehicle2box = dist_tmp;
                nearest_box = receive_boxes[i];
                have_real_box++;
            }
        }
        //publish nearest pose
        if(have_real_box>0)
        {
            geometry_msgs::Point pose_temp;
            pose_temp.x = (nearest_box.x_min+nearest_box.x_max)/2.0;
            pose_temp.y = (nearest_box.y_min+nearest_box.y_max)/2.0;
            pose_temp.z = (nearest_box.z_min+nearest_box.z_max)/2.0;
            tag_pose_pub.publish(pose_temp);
        }
        
    }
}

void circle_callback(const swarm_msgs::BoundingBoxes &msg)
{
    std::vector<Box> receive_boxes = deal_detection_to_3dpose(msg, "circle", REALSIZE_circle, FOCUS_d435, 100, R_d435i);
    if(receive_boxes.size()>0)
    {
        double min_dist_vehicle2box = 5.0;
        Box nearest_box;
        int have_real_box = 0;
        for(int i=0;i<receive_boxes.size();i++)
        {
            Eigen::Vector3d receive_center;
            receive_center(0) = (receive_boxes[i].x_min+receive_boxes[i].x_max)/2.0;
            receive_center(1) = (receive_boxes[i].y_min+receive_boxes[i].y_max)/2.0;
            receive_center(2) = (receive_boxes[i].z_min+receive_boxes[i].z_max)/2.0;
            double dist_tmp = sqrt((receive_center(0)-current_point.x)*(receive_center(0)-current_point.x)+
                                    (receive_center(1)-current_point.y)*(receive_center(1)-current_point.y)+
                                    (receive_center(2)-current_point.z)*(receive_center(2)-current_point.z));
            if(dist_tmp<2.0)
            {
                continue;
            }
            if(dist_tmp<min_dist_vehicle2box)
            {
                min_dist_vehicle2box = dist_tmp;
                nearest_box = receive_boxes[i];
                have_real_box++;
            }
        }
        //publish nearest pose
        if(have_real_box>0)
        {
            geometry_msgs::Point pose_temp;
            pose_temp.x = (nearest_box.x_min+nearest_box.x_max)/2.0;
            pose_temp.y = (nearest_box.y_min+nearest_box.y_max)/2.0;
            pose_temp.z = (nearest_box.z_min+nearest_box.z_max)/2.0;
            circle_pose_pub.publish(pose_temp);
        }
        
    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "lidar_detect");
    ros::NodeHandle nh;
    unionsys_core->detect_serial_number();

    nh.param<int>("image_width",image_width,1024);
    nh.param<int>("image_height",image_height,768);
    nh.param<double>("FOCUS_d435",FOCUS_d435,200);
    nh.param<double>("FOCUS_downcam",FOCUS_downcam,200);
    nh.param<double>("REALSIZE_circle",REALSIZE_circle,1.5);
    nh.param<double>("REALSIZE_tag",REALSIZE_tag,0.4);
    
    
    //Subscriber
    local_odom_sub = nh.subscribe("/Odometry",10,local_odom_callback);

    circle_yolo_sub = nh.subscribe("/det/circle",10,circle_callback);
    tag_yolo_sub = nh.subscribe("/det/tag",10,tag_callback);

    //Publisher
    marker_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_box", 1);
    marker_id_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_id_marker", 1);

    circle_pose_pub = nh.advertise<geometry_msgs::Point>("/zhou/pose_circle", 10);
    tag_pose_pub = nh.advertise<geometry_msgs::Point>("/zhou/pose_tag", 10);

    
    R_d435i<<1,0,0,
            0,1,0,
            0,0,1;
    double pith_angle = 90*deg2rad;
    R_downcam<<cos(pith_angle),0,sin(pith_angle),
          0,1,0,
          -sin(pith_angle),0,cos(pith_angle);


    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(200);
    //main loop here
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

std::vector<Box> deal_detection_to_3dpose(swarm_msgs::BoundingBoxes msg, std::string class_name, double real_size, double real_focus, int id_zone, Eigen::Matrix3d rotation_camera)
{
    std::vector<Box> box_queue_return;
    if(msg.bounding_boxes.size()>0)
    {
        int clusterId_draw = id_zone;
        for(int i=0;i<msg.bounding_boxes.size();i++)
        {
            clusterId_draw++;

            double box_h = msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin;
            double box_v = msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin;
            double box_size = sqrt(box_h * box_h + box_v * box_v);
            // if(box_size<20) //筛选太小的框
            // {
            //     continue;
            // }

            double Bias_H = image_width/2 -(msg.bounding_boxes[i].xmax+msg.bounding_boxes[i].xmin)/2.0 ; 
            double Bias_V = image_height/2 - (msg.bounding_boxes[i].ymax+msg.bounding_boxes[i].ymin)/2.0;
    
            double distance = real_size * sqrt(Bias_H*Bias_H + Bias_V*Bias_V + real_focus*real_focus) /  box_size;
            // std::cout<<"--------------------------------distance->"<<distance<<std::endl;
            double alpha = atan2(Bias_V,real_focus);
            double beta = atan2(Bias_H,real_focus);

            //std::cout<<"========>>self angle: "<<beta*180/3.14<<","<<alpha*180/3.14<<std::endl;
            mutual_odom(0) = distance * cos(alpha) * cos(beta);
            mutual_odom(1) = distance * cos(alpha) * sin(beta);
            mutual_odom(2) = distance * sin(alpha);

            // std::cout<<"---->camera body point: "<<mutual_odom(0)<<","<<mutual_odom(1)<<","<<mutual_odom(2)<<std::endl;
            Eigen::Matrix3d rotation_vehicle = current_angle_Q.toRotationMatrix(); 

            mutual_odom = rotation_camera * mutual_odom;      
            mutual_odom = rotation_vehicle * mutual_odom;
            
                     
            // Box vision_box;
            // vision_box.x_min = P_Cam(0)-real_size/2.0;
            // vision_box.y_min = P_Cam(1)-real_size/2.0;
            // vision_box.z_min = P_Cam(2)-real_size/2.0;
            // vision_box.x_max = P_Cam(0)+real_size/2.0;
            // vision_box.y_max = P_Cam(1)+real_size/2.0;
            // vision_box.z_max = P_Cam(2)+real_size/2.0;
            // vision_box.id = 0;
            // draw_box(vision_box, i, 0.2, 0, 0.5, 0.5);
            // std::cout<<"---->vehicle local point: "<<mutual_odom(0)<<","<<mutual_odom(1)<<","<<mutual_odom(2)<<std::endl;


            local_odom(0) = mutual_odom(0)+ current_point.x;
            local_odom(1) = mutual_odom(1)+ current_point.y;
            local_odom(2) = mutual_odom(2)+ current_point.z;

            // std::cout<<"--->camera predict pos : "<<vector_trans(0)<<","<<vector_trans(1)<<","<<vector_trans(2)<<std::endl;
            Box vision_box;
            vision_box.x_min = local_odom(0)-real_size/2.0;
            vision_box.y_min = local_odom(1)-real_size/2.0;
            vision_box.z_min = local_odom(2)-real_size/2.0;
            vision_box.x_max = local_odom(0)+real_size/2.0;
            vision_box.y_max = local_odom(1)+real_size/2.0;
            vision_box.z_max = local_odom(2)+real_size/2.0;
            vision_box.id = 0;
            draw_box(vision_box, clusterId_draw, 0.2, 0, 0.5, 0.5);
            draw_name(vision_box, class_name, clusterId_draw);
            box_queue_return.push_back(vision_box);
        }
    }
    return box_queue_return;
}


void draw_box(Box temp_box, int id, double width, double red, double green, double blue)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "target_box";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = id; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    line_strip.lifetime = ros::Duration(0.5);
    line_strip.scale.x = width; //width of the line
    line_strip.color.r = red; 
    line_strip.color.g = green; 
    line_strip.color.b = blue;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs:: Point p[8];
    p[0].x = temp_box.x_min;     p[0].y = temp_box.y_max;     p[0].z = temp_box.z_max;
    p[1].x = temp_box.x_min;     p[1].y = temp_box.y_min;     p[1].z = temp_box.z_max;
    p[2].x = temp_box.x_min;     p[2].y = temp_box.y_min;     p[2].z = temp_box.z_min;
    p[3].x = temp_box.x_min;     p[3].y = temp_box.y_max;     p[3].z = temp_box.z_min;
    p[4].x = temp_box.x_max;     p[4].y = temp_box.y_max;     p[4].z = temp_box.z_min;
    p[5].x = temp_box.x_max;     p[5].y = temp_box.y_min;     p[5].z = temp_box.z_min;
    p[6].x = temp_box.x_max;     p[6].y = temp_box.y_min;     p[6].z = temp_box.z_max;
    p[7].x = temp_box.x_max;     p[7].y = temp_box.y_max;     p[7].z = temp_box.z_max;
    //LINE_STRIP link 0-1 1-2 2-3 only
    for (int i = 0; i < 8; i++){
        line_strip.points.push_back(p[i]);
    }
    //so we need to add every links mannualy
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[3]);
    line_strip.points.push_back(p[2]);
    line_strip.points.push_back(p[5]);
    line_strip.points.push_back(p[6]);
    line_strip.points.push_back(p[1]);
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[7]);
    line_strip.points.push_back(p[4]);
    marker_pub.publish(line_strip);
    
    //end draw

    
}

void draw_name(Box temp_box,std::string name, int id)
{
    geometry_msgs::PoseStamped box_center;
    box_center.pose.position.x = (temp_box.x_min+temp_box.x_max)/2.0;
    box_center.pose.position.y = (temp_box.y_min+temp_box.y_max)/2.0;
    box_center.pose.position.z = (temp_box.z_min+temp_box.z_max)/2.0;

    //draw target name on box
    visualization_msgs::Marker marker_id;
    marker_id.header.frame_id="map";
    marker_id.header.stamp = ros::Time::now();
    marker_id.ns = "basic_shapes";
    marker_id.action = visualization_msgs::Marker::ADD;
    marker_id.pose.orientation.w = 1.0;
    marker_id.id =id;
    marker_id.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker_id.lifetime = ros::Duration(0.5);
    marker_id.scale.z = 0.3;
    marker_id.color.b = 0;
    marker_id.color.g = 0;
    marker_id.color.r = 255;
    marker_id.color.a = 1;
    geometry_msgs::Pose name_pose;
    name_pose.position.x =  box_center.pose.position.x;
    name_pose.position.y =  box_center.pose.position.y;
    name_pose.position.z =  temp_box.z_max;
    marker_id.text= name;
    marker_id.pose= name_pose;
    marker_id_pub.publish(marker_id);
}